#include "Homography.hpp"
#include <zMat.hpp>
#include <Utility/Log.hpp>
#include "../VisionTools.hpp"

namespace zzz{
bool Homography::Create(const vector<Vector2d> &pa, const vector<Vector2d> &pb) {
  ZCHECK_EQ(pa.size(), pb.size());
  zuint n=pa.size();
  zMatrix<double> A(Zeros<double>(n*2,9)),U,S,VT;
  zuint cur=0;
  for (zuint i = 0; i < n; ++i) {
    const Vector2d &a = pa[i], &b = pb[i];
    A(cur, 0) = a[0]; A(cur, 1) = a[1]; A(cur, 2) = 1; A(cur, 6) = -a[0] * b[0]; A(cur, 7) = -a[1] * b[0]; A(cur, 8) = -b[0];
    ++cur;
    A(cur, 3) = a[0]; A(cur, 4) = a[1]; A(cur, 5) = 1; A(cur, 6) = -a[0] * b[1]; A(cur, 7) = -a[1] * b[1]; A(cur, 8) = -b[1];
    ++cur;
  }
  if (!SVD(A,U,S,VT)) {
    ZLOGE<<"SVD ERROR!\n";
    return false;
  }

  Hab_(0, 0) = VT(8, 0); Hab_(0, 1) = VT(8, 1); Hab_(0, 2) = VT(8, 2);
  Hab_(1, 0) = VT(8, 3); Hab_(1, 1) = VT(8, 4); Hab_(1, 2) = VT(8, 5);
  Hab_(2, 0) = VT(8, 6); Hab_(2, 1) = VT(8, 7); Hab_(2, 2) = VT(8, 8);
  Hab_.Normalize();
  Hba_ = Hab_;
  if (!Hba_.Invert())
    return false;
  Hba_.Normalize();
  return true;
}

bool Homography::Create(const vector<Vector3d> &pa, const vector<Vector3d> &pb)
{
  ZCHECK_EQ(pa.size(), pb.size());
  zuint n = pa.size();
  vector<Vector2d> pa2,pb2;
  pa2.reserve(n);
  pb2.reserve(n);

  for (zuint i = 0; i < n; ++i) {
    pa2.push_back(FromHomogeneous(pa[i]));
    pb2.push_back(FromHomogeneous(pb[i]));
  }
  return Create(pa2, pb2);
}
}